using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;
using Microsoft.Xna.Framework;

namespace HappySprite.Simulation
{
    class Arbiter : IEquatable<Arbiter>
    {
        Contact contactPoint1;
        Contact contactPoint2;
        int contactPointCount;

        Body u;
        Body v;

        float friction;

        public Arbiter()
        {
        }

        public void Initialize(CollisionEventArgs e)
        {
            Body u = e.This;
            Body v = e.Other;

            contactPoint1 = new Contact();
            contactPoint2 = new Contact();

            if (u.GetHashCode() <= v.GetHashCode())
            {
                this.u = u;
                this.v = v;
                contactPoint1.Normal = e.Normal;
            }
            else
            {
                this.u = v;
                this.v = u;
                contactPoint1.Normal = -e.Normal;
            }

            contactPoint1.Position = e.ContactPoint;
            contactPoint1.Seperation = e.Depth;
            contactPointCount = 1;

            // The friction coefficient between the two bodies
            friction = (float)Math.Sqrt(u.Friction + v.Friction);
        }

        public bool Equals(Arbiter x)
        {
            return u == x.u && v == x.v;
        }

        public override int GetHashCode()
        {
            return u.GetHashCode() ^ v.GetHashCode();
        }

        public void ApplyImpulse()
        {
            ApplyImpulse(ref contactPoint1);
            if (contactPointCount == 2) ApplyImpulse(ref contactPoint2);
        }

        void ApplyImpulse(ref Contact c)
        {
            // Relative velocity at contact
            Vector2 dv = v.Velocity + Vector2Extensions.Cross(v.AngularVelocity, c.R2) - u.Velocity - Vector2Extensions.Cross(u.AngularVelocity, c.R1);

            dv = dv * 1.4f;

            // Compute normal impulse
            float vn = Vector2.Dot(dv, c.Normal);

            float dPn;
            if (World.SplitImpulses)
                dPn = c.NormalMass * (-vn);
            else
                dPn = c.NormalMass * (-vn + c.Bias);

            if (World.AccumulateImpulses)
            {
                // Clamp the accumulated impulse
                float Pn0 = c.AccumulatedNormalImpulse;
                c.AccumulatedNormalImpulse = MathHelper.Max(Pn0 + dPn, 0f);
                dPn = c.AccumulatedNormalImpulse - Pn0;
            }
            else
            {
                dPn = MathHelper.Max(dPn, 0f);
            }

            // Apply contact impulse
            Vector2 Pn = dPn * c.Normal;

            u.ApplyImpulse(-Pn, c.R1);
            v.ApplyImpulse(Pn, c.R2);

            if (World.SplitImpulses)
            {
                // Compute bias impulse
                dv = v.BiasedVelocity + Vector2Extensions.Cross(v.BiasedAngularVelocity, c.R2) - u.BiasedVelocity - Vector2Extensions.Cross(u.BiasedAngularVelocity, c.R1);
                float vnb = Vector2.Dot(dv, c.Normal);

                float dPnb = c.NormalMass * (-vnb + c.Bias);
                float Pnb0 = c.AccumulatedNormalImpulseBias;
                c.AccumulatedNormalImpulseBias = MathHelper.Max(Pnb0 + dPnb, 0f);
                dPnb = c.AccumulatedNormalImpulseBias - Pnb0;

                Vector2 Pb = dPnb * c.Normal;

                u.BiasedVelocity -= u.InverseMass * Pb;
                u.BiasedAngularVelocity -= u.InverseMomentOfIntertia * Vector2Extensions.Cross(c.R1, Pb);

                v.BiasedVelocity += v.InverseMass * Pb;
                v.BiasedAngularVelocity += v.InverseMomentOfIntertia * Vector2Extensions.Cross(c.R2, Pb);
            }

            // Relative velocity at contact
            dv = v.Velocity + Vector2Extensions.Cross(v.AngularVelocity, c.R2) - u.Velocity - Vector2Extensions.Cross(u.AngularVelocity, c.R1);

            dv = dv * 1.4f;

		    Vector2 tangent = new Vector2(c.Normal.Y, -c.Normal.X);
		    float vt = Vector2.Dot(dv, tangent);
		    float dPt = c.TangentMass * (-vt);

		    if (World.AccumulateImpulses)
		    {
			    // Compute friction impulse
			    float maxPt = friction * c.AccumulatedNormalImpulse;

			    // Clamp friction
			    float oldTangentImpulse = c.AccumulatedTangentImpulse;
			    c.AccumulatedTangentImpulse = MathHelper.Clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
			    dPt = c.AccumulatedTangentImpulse - oldTangentImpulse;
		    }
		    else
		    {
			    float maxPt = friction * dPn;
			    dPt = MathHelper.Clamp(dPt, -maxPt, maxPt);
		    }

		    // Apply contact impulse
		    Vector2 Pt = dPt * tangent;

            u.ApplyImpulse(-Pt, c.R1);
            v.ApplyImpulse(Pt, c.R2);
        }

        public void Merge(ref Arbiter old)
        {
            //Debug.Assert(contactPointCount == 1);

            //if (old.contactPointCount <= 0)
            //    return;

            //contactPoint2 = old.contactPoint1;
            //contactPointCount = 2;

            //if (!World.WarmStarting)
            //{
            //    contactPoint2.ImpulseNormal = 0f;
            //    contactPoint2.ImpulseNormalBias = 0f;
            //    contactPoint2.ImpulseTangent = 0f;
            //}
        }

        public void PreStep(float inverseDt)
        {
            float biasFactor = World.SplitImpulses ? 0.8f : 0.2f;
            biasFactor = World.PositionCorrection ? biasFactor : 0f;

            PreStep(ref contactPoint1, biasFactor, inverseDt);
            if (contactPointCount == 2) PreStep(ref contactPoint2, biasFactor, inverseDt);
        }

        void PreStep(ref Contact c, float biasFactor, float inverseDt)
        {
            const float allowedPenetration = 0.01f;

            c.R1 = c.Position - u.Transform.Position;
            c.R2 = c.Position - v.Transform.Position;

            // Precompute normal mass, tangent mass, and bias.
            float rn1 = Vector2.Dot(c.R1, c.Normal);
            float rn2 = Vector2.Dot(c.R2, c.Normal);

            float massNormalDenominator = u.InverseMass + v.InverseMass;

            massNormalDenominator += 
                u.InverseMomentOfIntertia * (Vector2.Dot(c.R1, c.R1) - rn1 * rn1) 
                + v.InverseMomentOfIntertia * (Vector2.Dot(c.R2, c.R2) - rn2 * rn2);

            c.NormalMass = 1f / massNormalDenominator;

            Vector2 tangent = new Vector2(c.Normal.Y, -c.Normal.X);
            float rt1 = Vector2.Dot(c.R1, tangent);
            float rt2 = Vector2.Dot(c.R2, tangent);

            float massTangentDenominator = u.InverseMass + v.InverseMass;

            massTangentDenominator += 
                u.InverseMomentOfIntertia * (Vector2.Dot(c.R1, c.R1) - rt1 * rt1) 
                + v.InverseMomentOfIntertia * (Vector2.Dot(c.R2, c.R2) - rt2 * rt2);

            c.TangentMass = 1f / massTangentDenominator;

            c.Bias = -biasFactor * inverseDt * MathHelper.Min(0f, c.Seperation + allowedPenetration);
        }
    }
}
